#include "observer.h" 

float Slo_A[2][2]={0,1,0,0},Slo_L[2][2]={1,0,0,1},Slo_g[2],Slo_D[2][2]={1,0,0,1},Slo_Fai[2]={1,1},Slo_F[2][2]={1,0,0,1};
float Slo_X[2],Slo_Y[2],Slo_e_y[2],Slo_V[2],Slo_efs;

void sliding_mode_observer(void)
{
	
	Slo_e_y[0]=Mag_Yaw-Slo_Y[0];
	Slo_e_y[1]=Gyr_rad.Z-Slo_Y[1];
	Slo_efs=sqrt((Slo_F[0][0]*Slo_e_y[0]+Slo_F[0][1]*Slo_e_y[1])*(Slo_F[0][0]*Slo_e_y[0]+Slo_F[0][1]*Slo_e_y[1])+(Slo_F[1][0]*Slo_e_y[0]+Slo_F[1][1]*Slo_e_y[1])*(Slo_F[1][0]*Slo_e_y[0]+Slo_F[1][1]*Slo_e_y[1]));
	Slo_V[0]=Slo_Fai[0]*(Slo_F[0][0]*Slo_e_y[0]+Slo_F[0][1]*Slo_e_y[1])/Slo_efs;
	Slo_V[1]=Slo_Fai[1]*(Slo_F[1][0]*Slo_e_y[0]+Slo_F[1][1]*Slo_e_y[1])/Slo_efs;
	Slo_g[1]=Gyr_rad.X*Gyr_rad.Y;
	
	Slo_X[0]+=Slo_X[1]+Slo_L[0][0]*Slo_e_y[0]+Slo_L[0][1]*Slo_e_y[1]+Slo_D[0][0]*Slo_V[0]+Slo_D[0][1]*Slo_V[1];
	Slo_X[1]+=Slo_L[1][0]*Slo_e_y[0]+Slo_L[1][1]*Slo_e_y[1]+Slo_D[1][0]*Slo_V[0]+Slo_D[1][1]*Slo_V[1]+Slo_g[1];
	Slo_Y[0]=Slo_X[0];
	Slo_Y[1]=Slo_X[1];
}


float Mag_Noise,Mag_Noise_A,Mag_Noise_AA;
float Observer_Yaw;
void Noise_Observer(void)
{
	ESO_ADRC(&ADRC_Test,Mag_Yaw);
	ESO_FstOrd_ADRC(&ADRC_Test1,Gyr_rad.Z*RadtoDeg);
	Mag_Noise=ADRC_Test.z3-ADRC_Test1.z2;
	Mag_Noise_A+=Mag_Noise*0.01f;
	Mag_Noise_AA+=Mag_Noise_A*0.01f;
	if(Mag_Noise*Mag_Noise>8000) Mag_Yaw=Mag_Yaw_1;  
	Hubu_Filter(Mag_Yaw, Gyr_rad.Z,&Observer_Yaw,0.025,RadtoDeg);
}	

